#include "bsp_Graysensor.h"

int Pin_Gray[4] = {FRONT_GRAY,BEHIND_GRAY,LEFT_GRAY,RIGHT_GRAY};
char *Mode_Gray_char[] = {"Front_Gray:", "       Behind_Gray:", "       Left_Gray:", "       Right_Gray:"};
int Mode_Gray[4] = {0};

void Graysensor_Init()
{
  pinMode(FRONT_GRAY, INPUT);
  pinMode(BEHIND_GRAY, INPUT);
  pinMode(LEFT_GRAY, INPUT);
  pinMode(RIGHT_GRAY, INPUT);
}
/**
 * @brief 打印灰度传感器的值到串口监视器上
 * @param static uint8_t* pMode
 * @return void
 * */
void printGray(int choice)
{
  selectGray(choice);

  for (int i = 0; i < 4; i++)
  {
    if (Mode_Gray[i])
    {
      Serial.print(*(Mode_Gray_char + i));
      Serial.print(analogRead(Pin_Gray[i]));
    }
  }
  Serial.println();
}

/**
 * @brief 读取selectGray指定灰度传感器的值，并返回距离出界已到达的百分比 
 * @param int* pmem  
 * @param int choice
 * @return int*
 * */
void getGray(float *pmem, int choice)
{
  int current = 0;
  int average[15][4];
  int sum[4]={0};
  
  selectGray(choice);

  
   for (int i = 0; i < ACCURACY ; i++)
   {
     for (int j = 0; j < 4; j++)
     {
       
      if (Mode_Gray[j])
      {
        average[i][current] =analogRead(Pin_Gray[j]) ;       //灰度越小 距离出界越近 100%代表出界

        sum[current] += average[i][current];                               //将每个值相加储存起来

        current++;
      }
     }
     current = 0;
   }

   for(int i = 0;i<4;i++)
   {
     pmem[i] = 1.0- (sum[i] / (float)ACCURACY /1023.0) ;
   }

}
/**
 * @brief 指定要操作的传感器
 * @param int choice
 * @return int
 * */
void selectGray(int choice)
{

  Mode_Gray[0] = (choice & 0b1000) >> 3;
  Mode_Gray[1] = (choice & 0b0100) >> 2;
  Mode_Gray[2] = (choice & 0b0010) >> 1;
  Mode_Gray[3] = (choice & 0b0001);

}
